
#include <algorithm/shared/utils/Vec3.h>
#include <utils/Format.h>
#include <utils/HasValue.h>
#include <utils/Utils.h>
#include <utils/Math.h>


#include <bit>
#include <cmath>


#include <boost/algorithm/string.hpp>

namespace solar
{

    auto Vec3::toString() const -> std::string
    {
        auto data = this->data();
        return ::solar::format("{}, {}, {}", data[0], data[1], data[2]);
    }

    auto Vec3::fromString(const std::string& input) -> Vec3
    {
        std::vector<std::string> split_strings;
        boost::split(split_strings, input, boost::is_any_of(" ,"), boost::token_compress_on);
        return {std::stod(split_strings[0]), std::stod(split_strings[1]),
                std::stod(split_strings[2])};
    }

    Vec3::Vec3(const vcl::Vec4d& vec) : internal_vector_(0, 0, 0, 0)
    {
        std::array<double, 3> double_array{};
        vec.store_partial(3, double_array.data());
        this->internal_vector_.load_partial(3, double_array.data());
    }

    [[nodiscard]] SOLAR_PURE_FUNCTION auto Vec3::x() const -> double
    {
        return this->internal_vector_[0];
    }

    [[nodiscard]] SOLAR_PURE_FUNCTION auto Vec3::y() const -> double
    {
        return this->internal_vector_[1];
    }

    [[nodiscard]] SOLAR_PURE_FUNCTION auto Vec3::z() const -> double
    {
        return this->internal_vector_[2];
    }

    void Vec3::set_x(double input_x) { this->internal_vector_.insert(0, input_x); }

    void Vec3::set_y(double input_y) { this->internal_vector_.insert(1, input_y); }

    void Vec3::set_z(double input_z) { this->internal_vector_.insert(2, input_z); }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto Vec3::operator-() const
        -> Vec3
    {
        return -this->internal_vector_;
    }

    auto HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION Vec3::operator+=(const Vec3& vec)
        -> Vec3
    {
        this->internal_vector_ += vec.internal_vector_;
        return *this;
    }

    auto HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION Vec3::operator-=(const Vec3& vec)
        -> Vec3
    {
        this->internal_vector_ -= vec.internal_vector_;
        return *this;
    }

    auto HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION Vec3::operator*=(const Vec3& vec)
        -> Vec3
    {
        this->internal_vector_ *= vec.internal_vector_;
        return *this;
    }

    auto HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION Vec3::operator/=(const Vec3& vec)
        -> Vec3
    {
        this->internal_vector_ /= vec.internal_vector_;
        return *this;
    }

    auto HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION Vec3::operator*=(const double& value)
        -> Vec3
    {
        this->internal_vector_ *= value;
        return *this;
    }

    auto HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION Vec3::operator/=(const double& value)
        -> Vec3
    {
        this->internal_vector_ /= value;
        return *this;
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto
    Vec3::squared_length() const -> double
    {
        return vcl::horizontal_add(vcl::square(this->internal_vector_));
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto Vec3::length() const
        -> double
    {
        return std::sqrt(squared_length());
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto
    Vec3::get_unit_vector() const -> Vec3
    {
        return this->internal_vector_ * const_expr::invSqrt(squared_length());
    }

    [[nodiscard]] SOLAR_PURE_FUNCTION auto Vec3::get_vec() const -> vcl::Vec4d
    {
        return this->internal_vector_;
    }

    [[nodiscard]] SOLAR_PURE_FUNCTION auto Vec3::data() const -> std::array<double, 3>
    {
        std::array<double, 3> result = {};
        this->internal_vector_.store_partial(3, result.data());
        return result;
    }

    [[nodiscard]] SOLAR_PURE_FUNCTION auto Vec3::to_string() const -> std::string
    {
        auto internal_data = data();
        return ::solar::format("({}, {}, {})", internal_data[0], internal_data[1],
                               internal_data[2]);
        // return "(" + std::to_string(internal_data[0]) + ", "
        //        +std::to_string(internal_data[1]) + ", "
        //        +std::to_string(internal_data[2]) + ")"  ;
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto
    operator+(const Vec3& vec1, const Vec3& vec2) -> Vec3
    {
        return vec1.get_vec() + vec2.get_vec();
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto
    operator-(const Vec3& vec1, const Vec3& vec2) -> Vec3
    {
        return vec1.get_vec() - vec2.get_vec();
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto
    operator*(const Vec3& vec1, const Vec3& vec2) -> Vec3
    {
        return vec1.get_vec() * vec2.get_vec();
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto
    operator/(const Vec3& vec1, const Vec3& vec2) -> Vec3
    {
        return vec1.get_vec() / vec2.get_vec();
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto
    operator*(const Vec3& vec1, const double& value) -> Vec3
    {
        return vec1.get_vec() * value;
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto
    operator*(const double& value, const Vec3& vec1) -> Vec3
    {
        return vec1.get_vec() * value;
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto
    operator/(const Vec3& vec1, const double& value) -> Vec3
    {
        return vec1.get_vec() / value;
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto dot(const Vec3& vec1,
                                                                              const Vec3& vec2)
        -> double
    {
        return vcl::horizontal_add((vec1 * vec2).get_vec());
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto
    unit_vector(const Vec3& vec) -> Vec3
    {
        return vec * const_expr::invSqrt(vec.squared_length());
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto cross(const Vec3& vec1,
                                                                                const Vec3& vec2)
        -> Vec3
    {
        // return Vec3(v1.y() * v2.z() - v1.z() * v2.y(),
        //         v1.z() * v2.x() - v1.x() * v2.z(),
        //         v1.x() * v2.y() - v1.y() * v2.x());

        auto temp_vec1 = vcl::permute4<1, 2, 0, -1>(vec1.get_vec());
        auto temp_vec2 = vcl::permute4<2, 0, 1, -1>(vec2.get_vec());
        auto temp_vec3 = vcl::permute4<2, 0, 1, -1>(vec1.get_vec());
        auto temp_vec4 = vcl::permute4<1, 2, 0, -1>(vec2.get_vec());
        return temp_vec1 * temp_vec2 - temp_vec3 * temp_vec4;
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto
    determinant(const Vec3& vec1, const Vec3& vec2, const Vec3& vec3) -> double
    {
        //     return (a.x() * (b.y() * c.z() - b.z() * c.y()) -
        //             a.y() * (b.x() * c.z() - b.z() * c.x()) +
        //             a.z() * (b.x() * c.y() - b.y() * c.x()));

        auto temp_vec1 = vcl::permute4<1, 0, 0, -1>(vec2.get_vec());
        auto temp_vec2 = vcl::permute4<2, 2, 1, -1>(vec3.get_vec());
        auto temp_vec3 = vcl::permute4<2, 2, 1, -1>(vec2.get_vec());
        auto temp_vec4 = vcl::permute4<1, 0, 0, -1>(vec3.get_vec());
        vcl::Vec4d sign_vec = {1, -1, 1, 0};
        return vcl::horizontal_add(sign_vec * vec1.get_vec() *
                                   (temp_vec1 * temp_vec2 - temp_vec3 * temp_vec4));
    }

    [[nodiscard]] HOT_FUNCTION SOLAR_PURE_FUNCTION FLATTEN_FUNCTION auto
    cramer(const Vec3& vec1, const Vec3& vec2, const Vec3& vec3, const Vec3& result) -> Vec3
    {
        //     double deter = determinant(a, b, c);
        //     double deter_1 = determinant(y, b, c);
        //     double deter_2 = determinant(a, y, c);
        //     double deter_3 = determinant(a, b, y);
        //     return Vec3(deter_1, deter_2, deter_3) / deter;

        auto deter = determinant(vec1, vec2, vec3);
        auto deter_1 = determinant(result, vec2, vec3);
        auto deter_2 = determinant(vec1, result, vec3);
        auto deter_3 = determinant(vec1, vec2, result);
        return Vec3(deter_1, deter_2, deter_3) / deter;
    }

    [[nodiscard]] SOLAR_PURE_FUNCTION auto Vec3::hasValue() const -> bool
    {
        auto vec = this->internal_vector_ - vcl::Vec4d{kUndefined, kUndefined, kUndefined, 0};
        vec *= vcl::Vec4d{1, 1, 1, 0};
        auto bits = std::popcount(vcl::to_bits(vcl::is_zero_or_subnormal(vec)));
        // return vcl::horizontal_add(vcl::is_zero_or_subnormal(vec));
        return bits < 4;
    }
} // namespace solar
